/******************************************************************************
WiiMotionPlus.cpp

For the STM32VLDISCOVERY evaluation board.                    (USD $11.85)

A libmaple program to read the Wii Motion Plus gyro's.
Uses anton's modified i2c.c and i2c.h libmaple programs for Discovery.

Wired to the Discovery hardware i2c port number two. (I2C2)
sda from Wii Motion Plus wired to Discovery PB11
scl from Wii Motion Plus wired to Discovery PB10
V+  from Wii Motion Plus wired to Discovery 3V
Gnd from Wii Motion Plus wired to Discovery GND

I used (2) 10K pullup resistors from 3V to sda and scl. Your mileage may vary.
A Nintendo factory Wii Motion Plus was used for development.

Modified from public code on April 20, 2011    By ridgebackred

This code is released with no strings attached.
******************************************************************************/
#include <stdio.h>
#include <stdlib.h>
#include "define.h"
#include "AQMath.h"
#include "wirish.h"
#include "i2c.h"
#include "FlightAngle.h"

//FlightAngle_ARG tempFlightAngle;
FlightAngle_MultiWii tempFlightAngle;
FlightAngle *flightAngle = &tempFlightAngle;
// Declare called routines
void InitWii(void);
void ReadWii(uint8 *buf);
void CalibrateGyros(void);
void ReadSerialCommand();
void sendSerialTelemetry() ;
 // Do housekeeping activities once at power-up********************************
  
void setup() {
 pinMode(BOARD_LED_PIN, OUTPUT);
// Serial3.begin(115200);
 pinMode(xPin, INPUT_ANALOG);
 pinMode(yPin, INPUT_ANALOG);
 pinMode(zPin, INPUT_ANALOG);
 InitWii();
 
 flightAngle->initialize(1.0, 0.0);
 previousTime = micros();
 RCinit();//无线初始化
 initMotor();//PWM输出
}


void loop() {
 
  currentTime = micros();
  deltaTime = currentTime - previousTime;
   if (deltaTime >= 10000) //100HZ
   {
      frameCounter++;
       if (frameCounter %   1 == 0) {  //  100 Hz tasks
      
       //SendDebugData();//debug
      
         G_Dt = (currentTime - hundredHZpreviousTime) / 1000000.0;
         hundredHZpreviousTime = currentTime;
         
     updateGyroData();   
     updateAccData();
      
      
    #ifdef FlightAngle_ARG_MODE
     flightAngle->calculate(RollGyroValue,                       \
                              PitchGyroValue,                      \
                              YawGyroValue,                        \
                              xValue,                              \
                              yValue,                               \
                              zValue,                               \
                              0.0,                                  \
                              0.0,                                   \
                              
                              0.0);
  #else 
    getInclination();//使用简易互补滤波
   // computeIMU();//multiwii
   #endif                              
                             
   }//end if (frameCounter %   1 == 0)
 
   if (frameCounter %   2 == 0) {  //  50 Hz tasks
    multiWiiserialCom();
   }
   if (frameCounter %   4 == 0) {  //  25 Hz tasks
     G_Dt = (currentTime - twentyFiveHZpreviousTime) / 1000000.0;
      twentyFiveHZpreviousTime = currentTime;
   }
   if (frameCounter %  10 == 0) {  //   10 Hz tasks
       G_Dt = (currentTime - tenHZpreviousTime) / 1000000.0;
      tenHZpreviousTime = currentTime;
    
     //  ReadSerialCommand();
     //  sendSerialTelemetry() ;
         toggleLED();
   }
    previousTime = currentTime;
}
  
if (frameCounter >= 100) 
      frameCounter = 0;
 

// delay(30);
 
}

